using System;
using System.Collections;
using UnityEngine;

public class PlanUpdateClosest : Plan
{

	private AgentBehaviourScript agent;
	private ArrayList herders;
	private ArrayList energyPickups;
	private Vector3 exitPosition;	
	
	private Plan searchPlan;
	private Maze maze;
	
	private int searchMode = 0;	
	
	private int currentHerderNum = 0;
	private int currentEnergyNum = 0;
	
	
	private float [] herderDistances;
	private float [] energyDistances;
	private float exitDistance;
	
	
	
	public PlanUpdateClosest(AgentBehaviourScript agent, Maze maze, ArrayList herders, ArrayList energyPickups, Vector3 exitPos)
	{
		this.maze = maze;
		this.agent = agent;
		this.herders = (ArrayList)herders.Clone();	
		//Array to keep track of distances to herder
		herderDistances = new float[this.herders.Count];
		this.energyPickups = (ArrayList)energyPickups.Clone();	
		//Array to keep track of distances to energy
		energyDistances = new float[this.energyPickups.Count];
		this.exitPosition = exitPos;
		
		for (int i = 0; i < herders.Count; i++)
		{
			herderDistances[i] = -1.0f;	
		}
		
		for (int i = 0; i < energyPickups.Count; i++)
		{
			energyDistances[i] = -1.0f;	
		}	
		
		//buildInitialDistances();
		
	}
	
	public override bool execute ()
	{
		for (int i = 0; i < herders.Count; i++)
		{
			try{
				Vector3 test = ((HerderBehaviourScript)herders[i]).getPosition();// Make sure herder still exists
			}catch
			{
				herderDistances[i] = -1.0f;	
			}
			
		}
		
		for (int i = 0; i < energyPickups.Count; i++)
		{
			try{
				Vector3 test = ((EnergyPickup)energyPickups[i]).getPosition();// Make sure energy still exists
			}catch
			{
				energyDistances[i] = -1.0f;	
			}
			
		}	
			
		if (searchPlan == null || !searchPlan.execute())	
		{
		
			if ((searchPlan != null) && searchPlan.getPathPlanner().is_goal_found())
			{
				switch(searchMode)
				{
				case 0:
						herderDistances[currentHerderNum] = searchPlan.getPathPlanner().get_path_length(); 
					break;
				case 1:
						energyDistances[currentEnergyNum] = searchPlan.getPathPlanner().get_path_length(); 
					break;
				}
			}
			
			if (searchMode == 2) // find Exit mode
			{
				if (searchPlan.getPathPlanner().is_goal_found())
				{
					exitDistance = searchPlan.getPathPlanner().get_path_length();	
				}else
				{
					exitDistance = 100000; // Long way away if we can't path find to it	
				}
			}
			
			searchMode++;
			if (searchMode >= 3) searchMode = 0;
			
			switch(searchMode)
			{
			case 0:
				startHerderSearch();	
				break;
			case 1:
				startEnergySearch();	
				break;
			case 2:
				searchPlan = new PlanBuildAStarPath( agent.getPosition(), maze, exitPosition, 1400, null);
				break;
			}
		}
		return false; // plan never finishes
	}
	
	private void startHerderSearch()
	{
		currentHerderNum++;
		if (currentHerderNum >= herders.Count)
		{
			currentHerderNum = 0;	
		}
		try{
			searchPlan = new PlanBuildAStarPath( agent.getPosition(), maze, ((HerderBehaviourScript)herders[currentHerderNum]).getPosition() , 1000, null);
		}catch{
			searchPlan = null;	
		}
	}
	
	private void startEnergySearch()
	{
		currentEnergyNum++;
		if (currentEnergyNum >= energyPickups.Count)
		{
			currentEnergyNum = 0;	
		}
		try{
			searchPlan = new PlanBuildAStarPath( agent.getPosition(), maze, ((EnergyPickup)energyPickups[currentEnergyNum]).getPosition(), 1000, null);
		}catch{
			searchPlan = null;	
		}
	}
	
	public float getExitDistance()
	{
		return exitDistance;	
	}
	
	public float getClosestEnergyDistance() {
		float closestDist = -1.0f;
		for(int i = 0; i < energyDistances.GetLength(0); i++)
		{
			if (energyDistances[i] >= 0.0f && (energyDistances[i] < closestDist || closestDist	< 0.0f))
			{
				closestDist = energyDistances[i];
			}
			
		}
		return closestDist;	
	}

	public float getClosestHerderDistance() {
		float closestDist = -1.0f;
		for(int i = 0; i < herderDistances.GetLength(0); i++)
		{
			if (herderDistances[i] >= 0.0f && (herderDistances[i] < closestDist || closestDist	< 0.0f))
			{
				closestDist = herderDistances[i];
			}
			
		}
		return closestDist;
	}
	
	public EnergyPickup getClosestEnergy()
	{
		float closestDist = -1.0f;
		EnergyPickup closestEnergy = null;
		for(int i = 0; i < energyDistances.GetLength(0); i++)
		{
			if (energyDistances[i] >= 0.0f && (energyDistances[i] < closestDist || closestDist	< 0.0f))
			{
				closestDist = energyDistances[i];
				closestEnergy = (EnergyPickup)energyPickups[i];
			}
			
		}
		return closestEnergy;	
	}
	
	public HerderBehaviourScript getClosestHerder()
	{
		float closestDist = -1.0f;
		HerderBehaviourScript closestHerder = null;
		for(int i = 0; i < herderDistances.GetLength(0); i++)
		{
			if (herderDistances[i] >= 0.0f && (herderDistances[i] < closestDist || closestDist	< 0.0f))
			{
				closestDist = herderDistances[i];
				closestHerder = (HerderBehaviourScript)herders[i];
			}
			
		}
		return closestHerder;
	}
	
	/*
	private void buildInitialDistances()
	{
		foreach (HerderBehaviourScript herder in herders)	
		{
			searchPlan = new PlanBuildAStarPath( agent.getPosition(), maze, herder.getPosition(), 6000, null);
			while(searchPlan.execute());
			updateHerder(herder, searchPlan.getPathPlanner().get_path_length());
		}
		
		foreach (EnergyPickup energy in energyPickups)	
		{
			searchPlan = new PlanBuildAStarPath( agent.getPosition(), maze, energy.getPosition(), 6000, null);
			while(searchPlan.execute());
			updateEnergy(energy, searchPlan.getPathPlanner().get_path_length());
		}
		
		searchPlan = null;
	}
	*/
	

		
	public override pathplanner getPathPlanner ()
	{
		if (searchPlan	!= null) {
			return searchPlan.getPathPlanner();
		} else {
			return null;
		}
	}

	public override string ToString ()
	{
		string returnString = string.Format ("Updating Distance( ");
		if (searchMode == 0)
			returnString += "Herder " + currentHerderNum + " - ";
		if (searchMode == 1)
			returnString += "Energy " + currentEnergyNum + " - ";
		if (searchMode == 2)
			returnString += "Goal2- ";
		if (searchPlan != null)
			returnString += searchPlan.ToString();
		returnString += " )";
		return returnString;
	}
	

	
}

